import { Stage, Container, Graphics } from "@createjs/easeljs";
import { ROS2D } from "./ros2d";
import { ROSLIB } from "./RosLib";
import EventEmitter2  from "eventemitter2";

var NAV2D = NAV2D || {
    REVISION : '0.3.0'
};

NAV2D.ImageMapClientNav = function(options,event) {
    var that = this;
    options = options || {};
    var image = options.image;
    this.message = options.message;
    this.rootObject = options.rootObject || new Container();
    this.viewer = options.viewer;
    this.withOrientation = options.withOrientation || false;

    // setup a client to get the map
    var client = new ROS2D.ImageMapClientWithMessage({
        rootObject : this.rootObject,
        image : image,
        message: this.message
    });
    
    client.on("change", function() {
        that.navigator = new NAV2D.Navigator({
            rootObject : that.rootObject,
            withOrientation : that.withOrientation,
        });
        that.navigator.on("add_maker",function(data){
            event("add_maker",data)
        })
        // scale the viewer to fit the map
        that.viewer.scaleToDimensions(client.currentImage.width, client.currentImage.height);
        that.viewer.shift(client.currentImage.pose.position.x, client.currentImage.pose.position.y);
    });
};

NAV2D.ImageMapClientNav.prototype.__proto__ = EventEmitter2.prototype;

NAV2D.Navigator = function(options) {
    var that = this;
    options = options || {};

    var withOrientation = options.withOrientation || false;
    this.rootObject = options.rootObject || new Container();

    var stage;
    if (that.rootObject instanceof Stage) {
        stage = that.rootObject;
    } else {
        stage = that.rootObject.getStage();
    }

    if (withOrientation === false){
        this.rootObject.addEventListener('dblclick', function(event) {
            var coords = stage.globalToRos(event.stageX, event.stageY);
            var pose = new ROSLIB.Pose({
              position : new ROSLIB.Vector3(coords)
            });
            that.emit('add_maker', [pose]);
        })
    } else {
        var position = null;
        var positionVec3 = null;
        var thetaRadians = 0;
        var thetaDegrees = 0;
        var orientationMarker = null;
        var mouseDown = false;
        var mouseMove = false;
        var xDelta = 0;
        var yDelta = 0;

        var mouseEventHandler = function(event, mouseState) {
            if (mouseState === 'down'){
                position = stage.globalToRos(event.stageX, event.stageY);
                positionVec3 = new ROSLIB.Vector3(position);
                mouseDown = true;
            } else if (mouseState === 'move'){
                that.rootObject.removeChild(orientationMarker);
                if ( mouseDown === true) {
                    mouseMove = true;
                    var currentPos = stage.globalToRos(event.stageX, event.stageY);
                    var currentPosVec3 = new ROSLIB.Vector3(currentPos);

                    orientationMarker = new ROS2D.ArrowShape({
                        size : 50,
                        strokeSize : 1,
                        fillColor : Graphics.getRGB(0, 255, 0, 0.66),
                        pulse : false
                    });
                    
                    xDelta =  currentPosVec3.x - positionVec3.x;
                    yDelta =  currentPosVec3.y - positionVec3.y;
                    
                    thetaRadians  = Math.atan2(xDelta,yDelta);
                    thetaDegrees = thetaRadians * (180.0 / Math.PI);
                            
                    if (thetaDegrees >= 0 && thetaDegrees <= 180) {
                        thetaDegrees += 270;
                    } else {
                        thetaDegrees -= 90;
                    }

                    orientationMarker.x =  positionVec3.x;
                    orientationMarker.y = -positionVec3.y;
                    orientationMarker.rotation = thetaDegrees;
                    orientationMarker.scaleX = 1.0 / stage.scaleX;
                    orientationMarker.scaleY = 1.0 / stage.scaleY;
                    
                    that.rootObject.addChild(orientationMarker);
                }
            } else if (mouseDown) {
                if(mouseMove) {
                    var goalPos = stage.globalToRos(event.stageX, event.stageY);

                    var goalPosVec3 = new ROSLIB.Vector3(goalPos);
                    
                    xDelta =  goalPosVec3.x - positionVec3.x;
                    yDelta =  goalPosVec3.y - positionVec3.y;
                    thetaRadians  = Math.atan2(xDelta,yDelta);
          
                    if (thetaRadians >= 0 && thetaRadians <= Math.PI) {
                        thetaRadians += (3 * Math.PI / 2);
                    } else {
                        thetaRadians -= (Math.PI/2);
                    }
                    var qz =  Math.sin(-thetaRadians/2.0);
                    var qw =  Math.cos(-thetaRadians/2.0);
                    
                    var orientation = new ROSLIB.Quaternion({x:0, y:0, z:qz, w:qw});
                    
                    var pose = new ROSLIB.Pose({
                        position :    positionVec3,
                        orientation : orientation
                    });
                    // send the goal
                    that.emit('add_maker', [pose]);
                }
                mouseMove = false;
                mouseDown = false;
            }
        }

        this.rootObject.addEventListener('stagemousedown', function(event) {
            mouseEventHandler(event,'down');
        });
    
        this.rootObject.addEventListener('stagemousemove', function(event) {
            mouseEventHandler(event,'move');
        });
    
        this.rootObject.addEventListener('stagemouseup', function(event) {
            mouseEventHandler(event,'up');
        });
    }

}

NAV2D.Navigator.prototype.__proto__ = EventEmitter2.prototype;

export { NAV2D }